perm filename TEST2.SAI[CRE,BGB] blob
sn#049890 filedate 1974-05-31 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00007 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00002 00002 BEGIN "TEST2"
C00004 00003 α IRON TRIANGLE - CAMERA LOCUS SOLVER
C00005 00004 SUBR LSCAM (ITG I,K,J)
C00006 00005 SUBR CAMINIT
C00009 00006 SUBR TRANSFORM
C00011 00007 α MAIN LOOP
C00012 ENDMK
C⊗;
BEGIN "TEST2"
REQUIRE "ABBREV[SYS,BGB]" SOURCE_FILE;
REQUIRE "DPYIII[SYS,BGB]" SOURCE_FILE;
REQUIRE "SAITRG[SYS,BGB]" SOURCE_FILE;
SAFE ITG ARRAY DPYBUF[0:999];
α CAMERA;
REAL PAN,TILT,SWING;
REAL CX,CY,CZ;
REAL IX,IY,IZ;
REAL JX,JY,JZ;
REAL KX,KY,KZ;
REAL SX,SY,FOCAL,MAG; α RASTER SCALES;
REAL LDX,LDY,PDX,PDY; α LOGICAL AND PHYSICAL RASTERS;
REAL XPA,YPA; α PRINCIPLE AXIS LOCUS IN PP;
α THREE INCH CUBE;
PRELOAD_WITH 0,3,3,3,0,0;SAFE REAL ARRAY XWC[1:6];
PRELOAD_WITH 0,0,0,3,3,3;SAFE REAL ARRAY YWC[1:6];
PRELOAD_WITH 0,0,3,3,3,0;SAFE REAL ARRAY ZWC[1:6];
PRELOAD_WITH '216,'176,'122,'105,'124,'200;SAFE ITG ARRAY ROW[1:6];
PRELOAD_WITH '220,'247,'251,'210,'160,'160;SAFE ITG ARRAY COL[1:6];
SAFE REAL ARRAY XPP,YPP,ZPP,XDC,YDC[1:6];
SAFE REAL ARRAY XXPP,YYPP,XXDC,YYDC[1:6];
SAFE REAL ARRAY XCC,YCC,ZCC[1:6];
α IRON TRIANGLE - CAMERA LOCUS SOLVER;
REAL ARRAY P1,P2,P3,COSANG[1:3],V[1:10,1:3];
REQUIRE "LS1V3P.REL" LOAD_MODULE;
EXTERNAL ITG PROCEDURE LS1V3P(REAL ARRAY V,P1,P2,P3,COSANG);
REAL SUBR DOTVEC(ITG I,J);
BEGIN "DOTVEC"
REAL X1,Y1,Z1,X2,Y2,Z2,R1,R2,ZCOS;
X1 ← XCC[I]; Y1 ← YCC[I]; Z1 ← ZCC[I];
X2 ← XCC[J]; Y2 ← YCC[J]; Z2 ← ZCC[J];
R1 ← SQRT(X1*X1 + Y1*Y1 + Z1*Z1);
R2 ← SQRT(X2*X2 + Y2*Y2 + Z2*Z2);
ZCOS←(X1*X2 + Y1*Y2 + Z1*Z2) / (R1*R2);
RETURN(ZCOS);
END "DOTVEC";
SUBR LSCAM (ITG I,K,J);
BEGIN "LSCAM"
ITG M,N;
α IRON TRIANGLE - KNOWN WORLD LOCI;
P1[1]←XWC[I]; P2[1]←XWC[J]; P3[1]←XWC[K];
P1[2]←YWC[I]; P2[2]←YWC[J]; P3[2]←YWC[K];
P1[3]←ZWC[I]; P2[3]←ZWC[J]; P3[3]←ZWC[K];
α IRON TRIPOD - KNOW ANGLES BETWEEN CAMERA RAYS;
COSANG[1] ← DOTVEC(J,K);
COSANG[2] ← DOTVEC(I,K);
COSANG[3] ← DOTVEC(I,J);
α THROW THE SHIT AT THE FAN;
M ← LS1V3P(V,P1,P2,P3,COSANG);
OUTSTR(9&CVS(M)&" CAMERA SOLUTIONS."&↓);
FOR N←1 TIL M DO
OUTSTR(9&CVG(V[N,1])&9&CVG(V[N,2])&9&CVG(V[N,3])&↓);
INCHRW;
END "LSCAM";
SUBR CAMINIT;
BEGIN "CAMINIT"
REAL RR;
REAL C_PAN,S_PAN,C_TILT,S_TILT,C_SWING,S_SWING;
CX ← -30; CY ← -24; CZ ← +22;
PAN ← -π/4; TILT ← 60*π/180; MAG ← 32/9;
FOCAL ← 25;
PDX ← 10.6; LDX ← 288;
PDY ← 8.6; LDY ← 216;
SX ← -FOCAL*LDX/PDX;
SY ← -FOCAL*LDY/PDY;
C_PAN ← COS(PAN); S_PAN ← SIN(PAN);
C_TILT ← COS(TILT); S_TILT ← SIN(TILT);
C_SWING ← COS(SWING); S_SWING ← SIN(SWING);
IX ← C_PAN*C_SWING - S_PAN*C_TILT*S_SWING;
IY ← S_PAN*C_SWING + C_PAN*C_TILT*S_SWING;
IZ ← S_TILT*S_SWING;
JX ← -C_PAN*S_SWING - S_PAN*C_TILT*C_SWING;
JY ← -S_PAN*S_SWING + C_PAN*C_TILT*C_SWING;
JZ ← S_TILT*C_SWING;
KX ← S_PAN*S_TILT;
KY ← -C_PAN*S_TILT;
KZ ← C_TILT;
END "CAMINIT";
SUBR TRANSFORM;
BEGIN "TRANSFORM"
ITG I;
FOR I←1 TIL 6 DO
BEGIN
REAL X,Y,Z;
α WC → CC WORLD LOCII;
X ← XWC[I] - CX;Y ← YWC[I] - CY;Z ← ZWC[I] - CZ;
XCC[I] ← X*IX + Y*IY + Z*IZ;
YCC[I] ← X*JX + Y*JY + Z*JZ;
ZCC[I] ← X*KX + Y*KY + Z*KZ;
α CC → PP;
XPP[I] ← SX * XCC[I] / ZCC[I];
YPP[I] ← SY * YCC[I] / ZCC[I];
α PP → RC;
α ROW[I] ← 108 - YPP[I]-YPA;
α COL[I] ← XPP[I] + 144 + XPA;
α RC → PP;
XPP[I] ← COL[I] - 144 - XPA;
YPP[I] ← 108 - ROW[I] - YPA;
α PP → CC IMAGE PLANE LOCII;
XCC[I] ← XPP[I]*FOCAL/SX;
YCC[I] ← YPP[I]*FOCAL/SY;
ZCC[I] ← FOCAL;
α PP → DC;
XDC[I] ← MAG * XPP[I];
YDC[I] ← MAG * YPP[I];
XXDC[I] ← MAG * (COL[I]-144);
YYDC[I] ← MAG * (108-ROW[I]);
END;
END "TRANSFORM";
SUBR SHOW;
BEGIN "SHOW"
ITG I;
DPYSET(DPYBUF);
AIVECT(-511,-384);
AVECT(+511,-384);
AVECT(+511,+384);
AVECT(-511,+384);
AVECT(-511,-384);
AIVECT(XDC[6],YDC[6]);
FOR I←1 TIL 6 DO AVECT(XDC[I],YDC[I]);
AIVECT(XXDC[6],YYDC[6]);
FOR I←1 TIL 6 DO AVECT(XXDC[I],YYDC[I]);
DPYOUT(1);
END "SHOW";
α MAIN LOOP;
CAMINIT;
TRANSFORM;
SHOW;
OUTSTR("*");INCHRW;
LSCAM(1,2,3);
LSCAM(1,2,4);
LSCAM(1,2,5);
LSCAM(1,2,6);
LSCAM(2,3,4);
LSCAM(2,3,5);
LSCAM(2,3,6);
LSCAM(3,4,5);
LSCAM(3,4,6);
LSCAM(4,5,6);
WHILE TRUE DO IF INCHRW="X" THEN DONE;
END "TEST2";